clear
boat_x = [0 1];                       % define the end points of the stick figure
boat_y = [0 0];
desiredH_x = [1 20];
desiredH_y = [0 0];

axis([-80.5 -79.5 40 41]);
hold on;
boatHDL=plot(boat_x,boat_y,'EraseMode','background'); % connect the end points,
                                               % and use "EraseMode" to REDUCE FLASHING
desiredHHDL = plot(desiredH_x, desiredH_y, '--','EraseMode','background'); 

headHDL = line('XData',boat_x(1), 'YData',boat_y(1), 'Color','b', ...
    'Marker','o', 'MarkerSize',6, 'LineWidth',2);
headTxt = text(boat_x(1), boat_y(1), sprintf('(%.3f,%.3f)',boat_x(1),boat_y(1)), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

goalHDL = line('XData',0, 'YData',0, 'Color','r', ...
    'Marker','d', 'MarkerSize',6, 'LineWidth',2);
goalTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

obsHDL = line('XData',0, 'YData',0, 'Color','k', ...
    'Marker','p', 'MarkerSize',6, 'LineWidth',2);
obsTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal
xlabel('Longitude');
ylabel('Latitude');
while 1
%    fp = fopen('~/autoBoat/log/status.txt');
%    c = fscanf(fp,'%s');
    c = urlread('http://192.168.1.102/status.txt');

    lat =  str2double(myParse(c, 'lat'));
    lon =  str2double(myParse(c, 'lon'));
    goalLat =  str2double(myParse(c, 'goalLat'));
    goalLon =  str2double(myParse(c, 'goalLon'));
    obsLat =  str2double(myParse(c, 'obsLat'));
    obsLon =  str2double(myParse(c, 'obsLon'));   
    compass = str2double(myParse(c, 'compass'));
    compass = compass/180*pi;
    desiredH = str2double(myParse(c, 'desiredHeading'));
    desiredH = desiredH/180*pi;
    boat_y=[lat-0.0001*cos(compass)  lat];
    boat_x=[lon-0.0001*sin(compass)  lon];
    desiredH_y = [lat lat+0.0001*cos(desiredH)];
    desiredH_x = [lon lon+0.0001*sin(desiredH)];
%    fclose(fp);
    set(boatHDL,'XData',boat_x,'YData',boat_y);       % REDUCE FLASHING
    set(headHDL, 'XData',lon, 'YData',lat);
    set(headTxt, 'Position',[lon lat], ...
        'String',sprintf('Boat(%.5f,%.5f)',[lon lat]));
    set(desiredHHDL,'XData',desiredH_x,'YData',desiredH_y);
    set(goalHDL, 'XData',goalLon, 'YData',goalLat);
    set(goalTxt, 'Position',[goalLon goalLat], ...
        'String',sprintf('Goal(%.5f,%.5f)',[goalLon goalLat]));   
    set(obsHDL, 'XData',obsLon, 'YData',obsLat);
    set(obsTxt, 'Position',[obsLon obsLat], ...
        'String',sprintf('Obstacle(%.5f,%.5f)',[obsLon obsLat]));
    drawnow         % forces Matlab to redraw the plot at the current values
    pause(.1)
    
end